#include "MainWindow.h"
#include "Environment.h"
#include "Car.h"
#include "Camera.h"

#include <QDockWidget>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <QSlider>
#include <QLCDNumber>

MainWindow::MainWindow(QWidget* parent)
    : QMainWindow(parent)
{
    // 初始化组件
    m_env = new Environment(this);
    m_car = new Car(40, 20);  // 40x20像素的小车
    m_camera = new Camera(m_env, m_car, this);
    
    setupUI();
    createControls();
    
    // 初始化小车位置
    m_env->addItem(m_car);
    m_car->setPos(-500, 0);
    m_car->setRotation(90);
    
    // 设置定时器（30FPS）
    m_timer = new QTimer(this);
    connect(m_timer, &QTimer::timeout, this, &MainWindow::updateSimulation);
    m_timer->start(33);  // ≈30Hz
}

void MainWindow::setupUI() {
    // 主视图配置
    m_mainView = new QGraphicsView(m_env);
    m_mainView->setRenderHint(QPainter::Antialiasing);
    m_mainView->setSceneRect(-500, -500, 1000, 1000);
    m_mainView->setMinimumSize(600, 400);
    
    // 摄像头视图
    m_cameraView = new QLabel;
    m_cameraView->setFixedSize(200, 150);
    m_cameraView->setFrameStyle(QFrame::Box | QFrame::Raised);
    
    // 布局设置
    QWidget* centralWidget = new QWidget;
    QHBoxLayout* mainLayout = new QHBoxLayout(centralWidget);
    mainLayout->addWidget(m_mainView, 3);
    
    QVBoxLayout* rightPanel = new QVBoxLayout;
    rightPanel->addWidget(m_cameraView);
    rightPanel->addStretch();
    mainLayout->addLayout(rightPanel, 1);
    
    setCentralWidget(centralWidget);
}

void MainWindow::createControls() {
    // 控制面板
    QWidget* controlPanel = new QWidget;
    QHBoxLayout* panelLayout = new QHBoxLayout(controlPanel);
    
    // 启停按钮
    QPushButton* startBtn = new QPushButton("Start", this);
    QPushButton* stopBtn = new QPushButton("Stop", this);
    connect(startBtn, &QPushButton::clicked, [=]{ m_timer->start(); });
    connect(stopBtn, &QPushButton::clicked, [=]{ m_timer->stop(); });
    
    // 速度显示
    QLCDNumber* speedLCD = new QLCDNumber(3);
    speedLCD->setSegmentStyle(QLCDNumber::Filled);
    connect(m_car, &Car::speedChanged, speedLCD, [=](qreal speed){
        speedLCD->display(static_cast<double>(speed));
    });    
    // 转向滑块
    QSlider* steeringSlider = new QSlider(Qt::Horizontal);
    steeringSlider->setRange(-100, 100);
    connect(steeringSlider, &QSlider::valueChanged, [=](int value){
        m_car->setSteering(value / 3.0);
    });
    
    // 添加到面板
    panelLayout->addWidget(startBtn);
    panelLayout->addWidget(stopBtn);
    panelLayout->addWidget(new QLabel("Speed:"));
    panelLayout->addWidget(speedLCD);
    panelLayout->addWidget(new QLabel("Manual Steering:"));
    panelLayout->addWidget(steeringSlider);
    
    // 将控制面板添加到主窗口底部
    QDockWidget* dock = new QDockWidget("Controls", this);
    dock->setWidget(controlPanel);
    addDockWidget(Qt::BottomDockWidgetArea, dock);
}

void MainWindow::updateSimulation() {
    // 1. 捕获摄像头画面
    QImage cameraImage = m_camera->captureView();
    
    // 2. 处理图像获取轨迹中心
    QPoint trackCenter = m_camera->processImage(cameraImage);
    QPixmap displayPix = QPixmap::fromImage(cameraImage);
    m_cameraView->setPixmap(displayPix);
    
    // 轨迹中心偏移量计算
    double theta_error = trackCenter.y();
    double distance_error = trackCenter.x();
    
    // PID控制参数
    double Kp_w = 0.03;
    double Kp_v = 0.35;

    // 计算角速度w
    double w = Kp_w * theta_error;
    
    // 控制速度v为常量
    double v = Kp_v * distance_error;
        // 限制角速度w的大小，假设最大角速度为MAX_W
    const double MAX_W = 0.2; // 根据实际情况调整最大角速度
    w = std::clamp(w, -MAX_W, MAX_W); // 限制角速度在 -MAX_W 到 MAX_W 之间
        
    // 输出调试信息
    std::cout << "car_v = " << v << std::endl;
    std::cout << "car_w = " << w << std::endl;
    
    // 设置车速和方向
    m_car->setSteering(w);
    m_car->setSpeed(v);
    
    // 5. 更新小车位置并保持视角居中
    m_car->updatePosition();
    m_mainView->centerOn(m_car);
    
    // 6. 刷新界面
    m_env->update();
}
